/*
 * motor.c
 *
 *  Created on: 26.2.2012
 *      Author: lukas
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include "motor.h"

uint8_t DIRECTION = 0;

volatile uint8_t SPEEDS_LEVEL2[10] = {0};
volatile uint8_t SPEEDS_LEVEL1[10] = {0};
volatile uint8_t SPEEDS_LEVEL1_COUNT = 0;

/*
 * Timer 0 (8bit) set to prescaler 256
 * on 8MHz -> 61,2 Hz
 * Phase correct PWM
 * Validated using VMLAB
 */
void MotorInit() {
	cli();

	// motor timer
	TCCR0 = (1 << WGM00) | (1 << COM01) | (1 << CS02);
	OCR0 = 0;
	DDRB |= (1 << PINB4) | (1 << PIN3) | (1 << PINB2);
	PORTB &= ~((1 << PINB4) | (1 << PIN3) | (1 << PINB2));

	// fan timer
	TCCR2 = (1 << WGM20) | (1 << COM21) | (1 << CS22) | (1 << CS21);
	OCR2 = 0;
	DDRD |= (1 << PIND7);
	PORTD &= ~(1 << PIND7);

	TIMSK |= (1 << TOIE2);

	sei();
}

/*
 * Set the motor speed and direction
 * speed 0..255
 * direction 0 = forward, 1 = backward
 * forward: PINB2 = 1, PINB4 = 0, backward: PINB2 = 0, PINB4 = 1
 */
void MotorSetSpeed(uint8_t speed, uint8_t direction) {
	OCR0 = 0;
	DIRECTION = direction;
	if (direction) { //
		PORTB |= (1 << PINB4);
		PORTB &= ~(1 << PINB2);
	} else {
		PORTB |= (1 << PINB2);
		PORTB &= ~(1 << PINB4);
	}
	OCR0 = speed;
	return;
}

void MotorSetFanSpeed(uint8_t fanSpeed) {
	OCR2 = fanSpeed;
}

uint8_t MotorGetDirection() {
	return DIRECTION;
}

uint8_t MotorGetSpeed() {
	return OCR0;
}

ISR(TIMER2_OVF_vect) {
	SPEEDS_LEVEL1[SPEEDS_LEVEL1_COUNT++] = OCR0;
	if (SPEEDS_LEVEL1_COUNT < 10) {
		return;
	}

	SPEEDS_LEVEL1_COUNT = 0;
	uint16_t sum = 0;
	for (int i = 0; i < 10; ++i) {
		sum += SPEEDS_LEVEL1[i];
	}
	uint8_t avgSpeed = sum / 10;

	for (int i = 9; i >= 0; --i) {
		SPEEDS_LEVEL2[i + 1] = SPEEDS_LEVEL2[i];
	}
	SPEEDS_LEVEL2[0] = avgSpeed;

	sum = 0;
	for (int i = 0; i < 10; ++i) {
		sum += SPEEDS_LEVEL2[i];
	}
	avgSpeed = sum / 10;
	MotorSetFanSpeed(avgSpeed);
}


